/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-11 13:52:35
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-13 11:26:30
 * @Description: ...
 * @Reference: ...
 */
#include "vrs_converter.h"

namespace kaist_publisher {

VrsConverter::VrsConverter(ros::NodeHandle &nh, const std::string &dataset_dir,
                           const std::string &save_dir,
                           const std::string &topic)
    : Converter(dataset_dir, save_dir), topic_(topic) {
    bag_name_ = FilterSlash(topic_) + ".bag";
    pub_vrs_ = nh.advertise<irp_sen_msgs::vrs>(topic_.c_str(), 10);
}

int64_t VrsConverter::Init_Publish() {
    const std::string data_file = dataset_dir_ + "/" + default_data_file;
    fp_ = fopen(data_file.c_str(), "r");

    if (fscanf(fp_,
               "%ld,%lf,%lf,%lf,%lf,%lf,%d,%d,%lf,%lf,%lf,%lf,%d,%lf,%lf,%lf,%"
               "c,%lf\n",
               &stamp_, &latitude_, &longitude_, &x_coordinate_, &y_coordinate_,
               &altitude_, &fix_state_, &number_of_sat_, &horizental_precision_,
               &lat_std_, &lon_std_, &altitude_std_, &heading_valid_,
               &heading_magnet_, &speed_knot_, &speed_km_, &GNVTG_mode_,
               &altitude_orthometric_) == 18) {
        return stamp_;
    } else {
        fclose(fp_);
        return LONG_MAX;
    }
}

int64_t VrsConverter::Publish() {
    irp_sen_msgs::vrs vrs_data;
    vrs_data.header.stamp.fromNSec(stamp_);
    vrs_data.header.frame_id = "altimeter";
    vrs_data.latitude = latitude_;
    vrs_data.altitude_orthometric = altitude_orthometric_;
    vrs_data.longitude = longitude_;
    vrs_data.x_coordinate = x_coordinate_;
    vrs_data.y_coordinate = y_coordinate_;
    vrs_data.altitude = altitude_;
    vrs_data.fix_state = fix_state_;
    if (fix_state_ == 1)
        vrs_data.fix_state_str = "normal";
    if (fix_state_ == 4)
        vrs_data.fix_state_str = "fix";
    if (fix_state_ == 5)
        vrs_data.fix_state_str = "float";
    vrs_data.number_of_sat = number_of_sat_;
    vrs_data.horizental_precision = horizental_precision_;
    vrs_data.lat_std = lat_std_;
    vrs_data.lon_std = lon_std_;
    vrs_data.altitude_std = altitude_std_;
    vrs_data.heading_valid = heading_valid_;
    vrs_data.heading_magnet = heading_magnet_;
    vrs_data.speed_knot = speed_knot_;
    vrs_data.speed_km = speed_km_;
    vrs_data.GNVTG_mode = GNVTG_mode_;

    // bag.write(topic_, vrs_data.header.stamp, vrs_data);
    pub_vrs_.publish(vrs_data);

    if (fscanf(fp_,
               "%ld,%lf,%lf,%lf,%lf,%lf,%d,%d,%lf,%lf,%lf,%lf,%d,%lf,%lf,%lf,%"
               "c,%lf\n",
               &stamp_, &latitude_, &longitude_, &x_coordinate_, &y_coordinate_,
               &altitude_, &fix_state_, &number_of_sat_, &horizental_precision_,
               &lat_std_, &lon_std_, &altitude_std_, &heading_valid_,
               &heading_magnet_, &speed_knot_, &speed_km_, &GNVTG_mode_,
               &altitude_orthometric_) == 18) {
        return stamp_;
    }

    fclose(fp_);
    return LONG_MAX;
}

void VrsConverter::Publish(int64_t &timestamp, irp_sen_msgs::vrsPtr &vrs_data) {
    vrs_data.reset(new irp_sen_msgs::vrs());
    vrs_data->header.stamp.fromNSec(stamp_);
    vrs_data->header.frame_id = "altimeter";
    vrs_data->latitude = latitude_;
    vrs_data->altitude_orthometric = altitude_orthometric_;
    vrs_data->longitude = longitude_;
    vrs_data->x_coordinate = x_coordinate_;
    vrs_data->y_coordinate = y_coordinate_;
    vrs_data->altitude = altitude_;
    vrs_data->fix_state = fix_state_;
    if (fix_state_ == 1)
        vrs_data->fix_state_str = "normal";
    if (fix_state_ == 4)
        vrs_data->fix_state_str = "fix";
    if (fix_state_ == 5)
        vrs_data->fix_state_str = "float";
    vrs_data->number_of_sat = number_of_sat_;
    vrs_data->horizental_precision = horizental_precision_;
    vrs_data->lat_std = lat_std_;
    vrs_data->lon_std = lon_std_;
    vrs_data->altitude_std = altitude_std_;
    vrs_data->heading_valid = heading_valid_;
    vrs_data->heading_magnet = heading_magnet_;
    vrs_data->speed_knot = speed_knot_;
    vrs_data->speed_km = speed_km_;
    vrs_data->GNVTG_mode = GNVTG_mode_;

    // bag.write(topic_, vrs_data->header.stamp, vrs_data);
    // pub_vrs_.publish(vrs_data);

    if (fscanf(fp_,
               "%ld,%lf,%lf,%lf,%lf,%lf,%d,%d,%lf,%lf,%lf,%lf,%d,%lf,%lf,%lf,%"
               "c,%lf\n",
               &stamp_, &latitude_, &longitude_, &x_coordinate_, &y_coordinate_,
               &altitude_, &fix_state_, &number_of_sat_, &horizental_precision_,
               &lat_std_, &lon_std_, &altitude_std_, &heading_valid_,
               &heading_magnet_, &speed_knot_, &speed_km_, &GNVTG_mode_,
               &altitude_orthometric_) == 18) {
        timestamp = stamp_;
        return;
    }

    fclose(fp_);
    timestamp = LONG_MAX;
    return;
}

int VrsConverter::Convert() {
    CheckAndCreateSaveDir();

    boost::filesystem::path bag_file =
        boost::filesystem::path(save_dir_) / bag_name_;
    // rosbag::Bag bag(bag_file.string(), rosbag::bagmode::Write);
    // ROS_INFO("saving %s", bag_file.c_str());

    const std::string data_file = dataset_dir_ + "/" + default_data_file;
    FILE *fp = fopen(data_file.c_str(), "r");
    irp_sen_msgs::vrs vrs_data;
    int64_t stamp;
    double latitude, longitude, altitude, altitude_orthometric;
    double x_coordinate, y_coordinate, horizental_precision, lat_std, lon_std,
        altitude_std, heading_magnet, speed_knot, speed_km;
    int fix_state, number_of_sat, heading_valid;
    char GNVTG_mode;
    while (fscanf(fp,
                  "%ld,%lf,%lf,%lf,%lf,%lf,%d,%d,%lf,%lf,%lf,%lf,%d,%lf,%lf,%"
                  "lf,%c,%lf\n",
                  &stamp, &latitude, &longitude, &x_coordinate, &y_coordinate,
                  &altitude, &fix_state, &number_of_sat, &horizental_precision,
                  &lat_std, &lon_std, &altitude_std, &heading_valid,
                  &heading_magnet, &speed_knot, &speed_km, &GNVTG_mode,
                  &altitude_orthometric) == 18) {
        vrs_data.header.stamp.fromNSec(stamp);
        vrs_data.header.frame_id = "altimeter";
        vrs_data.latitude = latitude;
        vrs_data.altitude_orthometric = altitude_orthometric;
        vrs_data.longitude = longitude;
        vrs_data.x_coordinate = x_coordinate;
        vrs_data.y_coordinate = y_coordinate;
        vrs_data.altitude = altitude;
        vrs_data.fix_state = fix_state;
        if (fix_state == 1)
            vrs_data.fix_state_str = "normal";
        if (fix_state == 4)
            vrs_data.fix_state_str = "fix";
        if (fix_state == 5)
            vrs_data.fix_state_str = "float";
        vrs_data.number_of_sat = number_of_sat;
        vrs_data.horizental_precision = horizental_precision;
        vrs_data.lat_std = lat_std;
        vrs_data.lon_std = lon_std;
        vrs_data.altitude_std = altitude_std;
        vrs_data.heading_valid = heading_valid;
        vrs_data.heading_magnet = heading_magnet;
        vrs_data.speed_knot = speed_knot;
        vrs_data.speed_km = speed_km;
        vrs_data.GNVTG_mode = GNVTG_mode;

        // bag.write(topic_, vrs_data.header.stamp, vrs_data);
        pub_vrs_.publish(vrs_data);
    }
    // bag.close();
    fclose(fp);
    // ROS_INFO("done saving %s", bag_file.c_str());

    return 0;
}

}  // namespace kaist_publisher
